#include <stdio.h>
#include "Nano100Series.h"
#include "Mmc3630.h"
#include "I2C1.h"

#define MIN(x,y) (x < y ? x : y)

#define MMC3630_I2C_ADDR		0x30
#define MMC3630_PID				0x0A

#define MMC3630KJ_REG_DATA			0x00
#define MMC3630KJ_REG_TEMP			0x06
#define MMC3630KJ_REG_STATUS		0x07
#define MMC3630KJ_REG_CTRL0			0x08
#define MMC3630KJ_REG_CTRL1			0x09
#define MMC3630KJ_REG_CTRL2			0x0A
#define MMC3630KJ_REG_SELFTEST		0x0E
#define MMC3630KJ_REG_PASSWORD		0x0F
#define MMC3630KJ_REG_OTPMODE		0x12
#define MMC3630KJ_REG_TESTMODE		0x13
#define MMC3630KJ_REG_OTP			0x2A
#define MMC3630KJ_REG_PRODUCTID		0x2F
 
#define MMC3630KJ_CMD_REFILL		0x20
#define MMC3630KJ_CMD_RESET         0x10
#define MMC3630KJ_CMD_SET			0x08
#define MMC3630KJ_CMD_TM_M			0x01
#define MMC3630KJ_CMD_TM_T			0x02
#define MMC3630KJ_CMD_PASSWORD		0xE1
#define MMC3630KJ_CMD_OTP_OPER		0x11
#define MMC3630KJ_CMD_OTP_MR		0x80
#define MMC3630KJ_CMD_OTP_ACT		0x80

static int is_mmc3630_exist = 0;
static uint16_t otpMatrix[3] = {1000, 1000, 1350};

static int Mmc3630Write8(uint8_t reg_addr, uint8_t reg_val)
{
	struct i2c_msg_t i2c_msg;
	int ret;
	uint8_t buf[2];

	buf[0] = reg_addr;
	buf[1] = reg_val;

	i2c_msg.addr = MMC3630_I2C_ADDR;
	i2c_msg.tx_buf = buf;
	i2c_msg.tx_to_write = 2;
	i2c_msg.tx_write = 0;
	i2c_msg.rx_buf = NULL;
	i2c_msg.rx_to_read = 0;
	i2c_msg.rx_read = 0;

	ret = I2c1Xfer(&i2c_msg);
	if (ret < 0) {
		return 0;
	} else {
		return i2c_msg.tx_write;
	}
}

static int Mmc3630Read(uint8_t reg_addr, uint8_t *reg_val, uint32_t n)
{
	struct i2c_msg_t i2c_msg;
	int ret;
	uint8_t buf[1];

	buf[0] = reg_addr;

	i2c_msg.addr = MMC3630_I2C_ADDR;
	i2c_msg.tx_buf = buf;
	i2c_msg.tx_to_write = 1;
	i2c_msg.tx_write = 0;
	i2c_msg.rx_buf = reg_val;
	i2c_msg.rx_to_read = n;
	i2c_msg.rx_read = 0;

	ret = I2c1Xfer(&i2c_msg);
	if (ret < 0) {
		return 0;
	} else {
		return i2c_msg.rx_read;
	}
}

static int Mmc3630Read8(uint8_t reg_addr, uint8_t *reg_val)
{
	return Mmc3630Read(reg_addr, reg_val, 1);
}

static void Mmc3630GetCompMtx(void)
{
	int16_t stemp = 0;
	uint16_t utemp = 0;
	uint8_t otpReg[2] = {0}; 

	Mmc3630Write8(MMC3630KJ_REG_PASSWORD, MMC3630KJ_CMD_PASSWORD);
	Mmc3630Write8(MMC3630KJ_REG_OTPMODE, MMC3630KJ_CMD_OTP_OPER);
	Mmc3630Write8(MMC3630KJ_REG_TESTMODE, MMC3630KJ_CMD_OTP_MR);  	
	Mmc3630Write8(MMC3630KJ_REG_CTRL2, MMC3630KJ_CMD_OTP_ACT); 	
	Mmc3630Read(MMC3630KJ_REG_OTP, otpReg, 2);
	
	otpMatrix[0] = 1000;							//magnify 1000 times

	stemp = otpReg[0] & 0x3F;
	if (stemp >= 32) {
		stemp = 32 - stemp;
	}
	otpMatrix[1] = stemp * 6 + 1000;	//*0.006*1000

	stemp =  (otpReg[0] & 0xC0) >> 6 | (otpReg[1] & 0x0F) << 2;
	if (stemp >= 32) {
		stemp = 32 - stemp;
	}
	utemp = stemp * 6 + 1000;		//magnify 1000 times
	otpMatrix[2] = utemp + (utemp * 3) / 10 + (utemp * 30 % 100 + utemp * 5) / 100;

	return;
}

void Mmc3630Init()
{
	uint8_t reg_val;
	int ret;

	reg_val = 0;
	ret = Mmc3630Read8(MMC3630KJ_REG_PRODUCTID, &reg_val);
	if ((ret == 0) || (reg_val != MMC3630_PID)) {
		is_mmc3630_exist = 0;
		LogDebug("can not find sensor mmc3630\n");
	} else {
		is_mmc3630_exist = 1;
		LogDebug("sensor id : 0x%02X\n", reg_val);
	}

	if(!is_mmc3630_exist) {
		return;
	}

	Mmc3630GetCompMtx();

	Mmc3630Write8(MMC3630KJ_REG_CTRL0, MMC3630KJ_CMD_REFILL);
	DelayMs(100);
	Mmc3630Write8(MMC3630KJ_REG_CTRL0, MMC3630KJ_CMD_SET);
	DelayMs(10);

	Mmc3630Write8(MMC3630KJ_REG_CTRL1, 0x00);		// ODR 100 Hz
	Mmc3630Write8(MMC3630KJ_REG_CTRL2, 1 << 0);		// 14 Hz
	Mmc3630Write8(MMC3630KJ_REG_CTRL0, MMC3630KJ_CMD_TM_M);
}

static void Mmc3630CompProcess(int32_t *in, int32_t *out)
{
	uint8_t i;
	int32_t temp[3] = {0};

	for (i = 0; i < 3; i++) {
		temp[i] = (int32_t)in[i] * otpMatrix[i] / 1000;

		if (temp[i] <= -32768) {
			temp[i] = -32768;
		} else if (temp[i] >= 32767) {
			temp[i] = 32767;
		}

		out[i] = temp[i];
	}

	return;
}

void Mmc3630GetMag(uint8_t *buf, uint32_t n)
{
	int ret;
	uint8_t reg_val;
	uint8_t buffer[6]={0};
	int32_t tempy, tempz;
	int32_t rawDataPre[3] = {0};
	int32_t rawDataOtp[3] = {0};
	float value[3];
	int32_t value_i[3];

	ret = Mmc3630Read8(MMC3630KJ_REG_STATUS, &reg_val);
	//LogDebug("MMC3630_REG_ADDR_STATUS : 0x%02X\n", reg_val);

	ret = Mmc3630Read(MMC3630KJ_REG_DATA, buffer, 6);
	if (ret < 6) {
		LogDebug("MMC3630_REG_ADDR_XOUT_LOW error\n");
		return;
	}

	rawDataPre[0] = (buffer[1] << 8) | buffer[0];
    rawDataPre[1] = (buffer[3] << 8) | buffer[2];
    rawDataPre[2] = (buffer[5] << 8) | buffer[4];

	rawDataPre[0] = rawDataPre[0] - 32768;
	tempy = rawDataPre[1] - rawDataPre[2] + 32768 - 32768;
    tempz = rawDataPre[1] + rawDataPre[2] - 32768 - 32768;
    rawDataPre[1] = tempy;
    rawDataPre[2] = tempz;

    Mmc3630CompProcess(rawDataPre, rawDataOtp);

	value[0] = (float)(rawDataOtp[0]) / 1024 * 100;   //the value is Gauss unit
    value[1] = (float)(rawDataOtp[1]) / 1024 * 100;   //1024: sensitivity
    value[2] = (float)(rawDataOtp[2]) / 1024 * 100;

	//LogDebug("value x : %f, y : %f, z : %f\n", value[0], value[1], value[2]);

	value_i[0] = value[0] / 1300 * 4096;		//13bit
	value_i[1] = value[1] / 1300 * 4096;
	value_i[2] = value[2] / 2500 * 16384;	// 15bit

	value_i[0] <<= 3;
	value_i[1] <<= 3;
	value_i[2] <<= 1;

	buf[0] = value_i[0] & 0xFF;
	buf[1] = (value_i[0] >> 8) & 0xFF;
	buf[2] = value_i[1] & 0xFF;
	buf[3] = (value_i[1] >> 8) & 0xFF;
	buf[4] = value_i[2] & 0xFF;
	buf[5] = (value_i[2] >> 8) & 0xFF;
}
